COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	THIS FILE IS INTERNAL DOCUMENTATION NOT FOR PUBLICATION.
C00003 00003		DEVICES
C00013 00004		IMPLEMENTATION OF ON-MONITORS
C00016 00005		INTERPRETER CODE FOR MOTIONS
C00018 00006		THE INTERPRETER SCHEDULER
C00028 00007	SWING-TIME Calculation of set points.
C00030 ENDMK
C⊗;
THIS FILE IS INTERNAL DOCUMENTATION NOT FOR PUBLICATION.
Authors: RF and LOU.

	DEVICES

	One does not request that a joint or a screwdriver start up;
one requessts that a device be started.  A device has the following
format:
	JCOUNT	Number of joints in the device
	RCOUNT	Number of running joints at the moment
	JOINT 	Points to joint control block for one joint
	STATUS	Status of this joint.  Includes these bits:
	    STARTED	This joint has been started up
	    RUNNING	This joint currently in motion
	    TERMINATED  This joint finished normally
	    ERROR	This joint had an error condition
	    STOPPED	Someone had to stop this joint.
	These last two fields, JOINT and STATUS are repeated
	for each joint.

	A joint control block has these fields:
	THETA	Current joint angle
	TORQUE	Current torque
	DRIVE	Current drive
	TRAJ	Pointer to the trajectory currently being followed

The way that an interpreter is expected to accomplish a move is
as follows:

0)	Execute a "wait-enable" for each on-monitor which is
to run during the motion.  These monitors will be started up
at the time the motion proper starts.

1)	Execute the "prepare move" instruction, which points to
a trajectory file (to be described) and causes modifications to
be made.

2)	Set up a device which includes all the joints of interest.
Cause the device to begin executing.  All this is done with the
instruction "start move" which takes as argument a bit pattern
encoding the joints to include in the device.  Start move causes
the servos to be scheduled.
Note that "start move" acts logically very much like a sprout;
the coming wait instruction will wait on this device.

3)	Execute a "wait" instruction.
This causes all the prepared wait-type on-monitors to be started, and
the interpreter itself is put
in a wait state which will terminate when the device is finished.
All the on-montors active for the motion will also be disabled before
the interpreter is called back.




Termination  of a move is quite simple:  The joint status word, which
is global to the entire runtime, is just set for each joint  affected
to  a value  which  means "unrunnable"  The  next time  the servo  is
awakened,  it  will  notice  this   and  request  euthanasia.     The
termination also requires that the motors be  stopped, and the brakes
applied, and the resting point of the arm determined.  

Some care must be taken to prevent two interpreters proceeding at the
same time  under conditions whereby one should  really be waiting for
the other.    Specifically,  during  a  move,  the  interpreter  that
initiated that move is suspended until it is complete.  Therefore, if
an  on-test  causes  another  interpreter  to  start,  the  suspended
interpreter must remain suspended until the new one dies.
TRAJECTORY TABLES

The following are necessary properties of trajectory tables:
	Must state which frames are used, so the servo can do appropriate 
		update.
	Must be perspicuous enough to allow servo to find the things which
		it must update.
	Must allow escape to interpret mode at end of each segment for
		each joint, in order to initiate new motions or "on" tests.

	A trajectory table  is a set  of joint-segment tables.   Each
joint-segment table is intended to give instructions for the servoing
of one joint along one segment of  the path.  There are two types  of
such tables: location  and inertia.  The servo follows  each of these
simultaneously and asynchronously.


FORMAT OF THE  LOCATION-JOINT-SEGMENT TABLE: This table is  ten words
long.

words 1,2,3,4,5,6:  coefficients a5,a4,a3,a2,a1,a0 of the trajectory.
Polynomial is normalised for tε[0,1].

word 7: Duration of segment in milliseconds.

word 8: Pointer to next location-joint-segment table for  this joint.
If this is the last segment, word 8 is 0.

word  9: Pointer  to frame  which is  to be  assumed at  end of  this
segment.  If no particular frame, then this is  0.  This is used in a
preprocessing step to modify  the coefficients of the  polynomial; at
swing time it is not needed.

word 10:  If not 0,   then a  pointer to a  location of interpretable
code where an interpreter is to  be instantiated and begun.  This  is
for the THEN part of VIA lists.

FORMAT OF  THE INTERTIA-JOINT-SEGMENT TABLE:  This table  is 7  words
long.

words 1,2: Coefficients j1,j0 of joint inertia polynomial, normalized
for tε[0,1].

words  3,4:  Coefficients  g1,g0   of  gravity  loading   polynomial.
Likewise normalized.

word 5: Duration of segment in milliseconds.

word 6: Control word, with bits for free, force, drive, nodrive, etc.
see later  discussion on control words.  This is copied into a global
location at start of inertia-joint-segment.

word 7: Pointer to next intertia-joint-segment table  for this joint.
If this is the last segment, word 7 is 0.


GO TABLES

The following are necessary properties of go tables:
	Must state list of frames to use (including initial deproach
and via-list)
	Must allow escape to interpret mode at end of each segment.

	A GO table is a list of frames through which the motion is to
proceed.  Each is specified by its address.  The first one is the deproach
of the start frame; the penultimate is the deproach of the final
frame.  Following each frame address is an interpreter address, which
is the adrress of the interpreter (if any) to be invoked upon achievement
of the preceeding frame.  The last frame is distinguished by an interpreter
address of -1.
	IMPLEMENTATION OF ON-MONITORS
This page is a set of notes for the eventual documentation.
Author:  RF

	The code for an on-monitor is in-line and jumped around.
The code is PDP11 code, including calls on arithmetic subroutines,
if neccessary.

Each on-monitor is given a unique name by the compiler.
The scheduler associates that name with the location of the
enable bit for this monitor.

On-monitors have only two states:  enabled and disabled.
There is a status word associated with each on-monitor;
it contains two bits:  the enabled, and the kill.

At the start of a program, all on-monitors are disabled.
That means enable=0, kill=0.

At the end of a block, the compiler includes code to disable all
on-monitors within that block.

Only one copy of an on-monitor ever exists.  No multiple instantiations.

ENABLE <NAME>
To enable a software monitor, 
tell kernel to activate it at its address, in on-level, where
it will have PDP11 code to:
1) look at its "enabled bit".  If already on, dismiss. If not, turn on.
2) clear all its working buffers (if any) 
3) schedule its second entry (the monitor itself) to be awakened at
the appropriate intervals.  Then dismiss.

The software monitor, every time it is awakened, does the following:
1)  look at its "kill bit".  If on, clear and dismiss.
2)  execute the code for its check.
3)  if condition satisfied, do immediate stuff,
 instantiate the conclusion, and dismiss.
4)  reschedule self at appropriate interval.  Dismiss.

To enable a hardware monitor,
tell kernel to activate it at its address, in on-level, where
it will have PDP11 code to:
1)  look at the device enabled bit.  If already on, return.  If not, turn on.
2)  dismiss.

DISABLE <NAME>
To disable a software monitor, set its  "kill bit".
To disable a hardware monitor, clear its "enabled bit".
	INTERPRETER CODE FOR MOTIONS
not yet ready.  Author:  RF.

The code generated for a move includes the following interpreter commands:

1) Prepare move.  This points to the move table, and causes the trajectory
to be modified to conform to current locations of via points.  Joint
loading is calculated for each via point as well.

2) Enable on-monitor.  There may be more than one of these instructions.

3) Start motion.  This points to the modified move table.

4) Await completion.  This causes the interpreter to wait until the
move has completed, or the arm has been stopped for some reason.
The signal which awakes the interpreter comes from whichever joint
servo finishes last: each servo, as it finishes, checks to see if all
the others are done, and if so, awakes the interpreter at servo level.

5) disable all on-monitors associated with move.  This is done at
servo level

6) dismiss to interpreter level.

Form of trajectory output to 11:

TRAJECTORY
	JOINT LIST
	NUMBER SEGS
	VIA LIST

JOINT
	JOINT DRIVE BLOCK
	NEXT
	SEGMENT LIST

SEGMENT	
	C0
	C1
	C2
	C3
	C4
	NEXT

VIA
	FRAME
	DURATION
	NEXT
	INTERPRETER

	THE INTERPRETER SCHEDULER

	Interpreters are managed by the following set of routines
which they can call.  These routines provide a facility for
sprouting new interpreters, entering a wait state until all
descendents have terminated, and terminating.  Furthermore,
interpreters can queue ON-monitors to be run during the
next wait.  Standard ON-monitors (those which are intended
to be run during waking life) are treated directly, and not
by these routines.  It is the responsibility of each process
to disable all ON-monitors of the waking variety as blocks
are exited; this is beyond the realm of the following routines.
	Interpreters run at processor level 1 in supervisor mode.
Sprouting a new interpreter is equivalent to queuing it in the
level 1 queue; terminating is equivalent to dismissing.  The level 1
interrupt handler upon awakening removes the process at the head
of the queue, and even if the queue is not empty, leaves the interrupt
set.  This may sound wrong, but it has the effect that an active
interpreter will never be interrupted by another interpreter.  If
the interrupt handler finds nothing to do at all, then and only
then does it clear the interrupt.  It jumps directly to the
interpreter which was removed from the queue.
	The only cumbersome feature is entering wait.  All
stack information is copied into a safe place, all wait-type
on-monitors are activated.  Each time a process terminates,
it is necessary to see if the mother can be aroused from her
sleep.  When finally she wakes up, all her wait-monitors must
be disabled.

RCLASS ONID (INTEGER STATUS, OPC; RPTR (ONID) NEXT);
	COMMENT:  STATUS holds the Kill and Enable bits.
	OPC is the On-program counter.
	NEXT is used for chaining of ons;

RCLASS PROCESSID (INTEGER PC, DEPENDENTS, WAITTYPE;  
	RPTR(PROCESSID) NEXT, MOTHER; RPTR(ONID) WAITONS);
	COMMENT:  PC=program counter.  DEPENDENTS=count.
	WAITTYPE=0 for run, 1 for dependent wait, 2 for event wait.
	NEXT used for queuing.  MOTHER points to mother.
	WAITONS is list of dependent on-monitors active only during wait;

RCLASS QUEUE (RPTR(PROCESSID) HEAD, TAIL);
RPTR (PROCESSID) CURRENT;
RPTR(QUEUE) RUNQUEUE;
DEFINE ENABLE = "'1";
DEFINE KILL = "'2";

PROCEDURE INITIALIZE;
	BEGIN
	Comment:  Establish queue for level 1;
	QUEUE:HEAD(RUNQUEUE) ← NEW_RECORD (PROCESSID);
	Comment:  Establish onid's for each on-monitor;
	FOREACH ON-MONITOR DO
		BEGIN
		X ← NEW_RECORD (ONID);
		ONID:NEXT(X) ← RNULL;
		ONID:STATUS(X) ← 0;
		ONID:OPC(X) ← OPC;
		END;
	END;

PROCEDURE ENQUEUE(RPTR(QUEUE) WHERE; RPTR(PROCESSID) NEW);
	BEGIN
	COMMENT:  Puts a process on a queue.  QUEUE tells which
		queue.  NEW is the process id;
	QUEUE:TAIL(WHERE) ← PROCESSID:NEXT(QUEUE:TAIL(WHERE)) ← NEW;
	PROCESSID:NEXT(QUEUE:TAIL(WHERE)) ← NULL;
	END;

RPTR(PROCESSID) PROCEDURE DEQUEUE (RPTR(QUEUE) WHERE);
	BEGIN
	COMMENT:  Returns head of queue, which is removed;
	RPTR(PROCESSID) ANSWER;
	IF QUEUE:HEAD(WHERE) = QUEUE:TAIL(WHERE) 
	THEN	RETURN(NULL);  COMMENT:  Empty queue;
	ANSWER ← PROCESSID:NEXT(QUEUE:HEAD(WHERE));
	IF (PROCESSID:NEXT(QUEUE:HEAD(WHERE)) ← PROCESSID:NEXT(ANSWER)) = NULL
	THEN	QUEUE:TAIL(WHERE) ← QUEUE:HEAD(WHERE);
	RETURN(ANSWER);
	END;

PROCEDURE SPROUT (IPC);
	BEGIN
	COMMENT: IPC is the interpreter program counter.
	The effect is to put the new interpreter in the run-queue;
	RPTR (PROCESSID) NEW;
	NEW ← NEW_RECORD (PROCESSID);
	PROCESSID:PC(NEW) ← IPC;
	PROCESSID:DEPENDENTS(NEW) ← 0;
	PROCESSID:WAITTYPE(NEW) ← 0;
	PROCESSID:MOTHER(NEW) ← CURRENT;
	PROCESSID:DEPENDENTS(CURRENT) ← PROCESSID:DEPENDENTS(CURRENT) + 1
	ENQUEUE(RUNQUEUE,NEW);
	<GENERATE LEVEL 1 INTERRUPT>;
	END;

PROCEDURE TERMINATE;
	BEGIN  COMMENT:  Terminates the caller;
	RPTR(PROCESSID) MOM;
	RPTR(ONID) VICTIM, OLD;
	INTEGER TEMP;
	IF PROCESSID:DEPENDENTS(CURRENT) ≠ 0
	THEN 	BEGIN  COMMENT:  This should never happen;
		ERROR("ACTIVE PROCESS DEPENDS ON BLOCK BEING EXITED");
		WAIT;
		END;
	MOM ← PROCESSID:MOTHER(CURRENT);
	TEMP ← PROCESSID:DEPENDENTS(MOM) ← PROCESSID:DEPENENTS(MOM) - 1;
	IF PROCESSID:WAITTYPE(MOM)=1 AND TEMP=0
	THEN	BEGIN  COMMENT:  Must awaken mom;
		VICTIM ← PROCESSID:WAITONS(MOM);
		PROCESSID:WAITONS(MOM) ← NULL;
		WHILE VICTIM ≠ NULL DO
			BEGIN  COMMENT:  Disable mom's wait ons;
			DISABLE(VICTIM);
			OLD ← VICTIM;
			VICTIM ← ONID:NEXT(VICTIM);
			ONID:NEXT(OLD) ← NULL;
			END;
		PROCESSID:WAITTYPE(MOM) ← 0;
		ENQUEUE(RUNQUEUE,MOM);
		END;
	CURRENT ← NULL;  COMMENT:  explicit deallocate?;
	<DISMISS LEVEL 1>;
	END;

PROCEDURE WAIT;
	BEGIN  COMMENT: Caller wishes to wait until all descendents have dismissed;
	RPTR(ONID) DEB;
	IF PROCESSID:DEPENDENTS(CURRENT) ≠ 0
	THEN	BEGIN  COMMENT:  Must wait;
		PROCESSID:PC(CURRENT) ← <RETURN ADDRESS>;
		PROCESSID:WAITTYPE(CURRENT) ← 1;
		DEB ← PROCESSID:WAITONS(CURRENT);
		WHILE DEB ≠ NULL DO
			BEGIN  COMMENT:  Enable all wait ons;
			IF ONID:STATUS(DEB) LAND ENABLE = 0
			THEN	BEGIN
				ONID:STATUS(DEB) ← ONID:STATUS(DEB) LAND ¬KILL;
				PUSHJ (ONID:OPC(DEB));
				Comment:  That causes it to reschedule itself;
				END;
			DEB ← NEXT(DEB);
			END;
		<DISMISS LEVEL 1>;
		END;
	END;

PROCEDURE WAIT_ENABLE (RPTR (ONID) MON);
	BEGIN  COMMENT:  Wish to set up a wait on-monitor;
	IF ONID:NEXT(MON) = NULL 
	THEN	BEGIN  COMMENT:  Add this wait on to list for Current;
		ONID:NEXT(MON) ← PROCESSID:WAITONS(CURRENT);
		PROCESSID:WAITONS(CURRENT) ← ONID;
		END;
	END;

PROCEDURE LEVEL_ONE_INTERRUPT_HANDLER;
	BEGIN
	IF RUNQUEUE = NULL THEN
		BEGIN  COMMENT:  No one left to run;
		<CLEAR INTERRUPT>;
		<DISMISS LEVEL 1>;
		END
	ELSE	BEGIN
		CURRENT ← DEQUEUE(RUNQUEUE);
		PUSH NAME(CURRENT);
		PUSH IPC(CURRENT);
		PUSHJ INTERPRETER;
		END;
	END;

Comment:  The following are not part of scheduling;

PROCEDURE RUN_ENABLE (RPTR (ONID) MON);
	BEGIN  COMMENT:  Wish to enable a run on monitor;
	↑;
	IF ONID:STATUS(MON) LAND ENABLE ≠ 0
	THEN	BEGIN ↓; RETURN END;  COMMENT:  Already going;
	ONID:STATUS(MON) ← ONID:STATUS(MON) LAND ¬KILL;
	PUSHJ (ONID:OPC(MON));
	Comment:  This causes it to reschedule itself in future;
	END;

PROCEDURE DISABLE (RPTR(ONID) MON);
	BEGIN  
	ONID:STATUS(MON) ← KILL;
	END;

SWING-TIME Calculation of set points.

Due to the run-time modification of trajectories and sloppiness
in the compiler, trajectories will occasionally end up  requesting
joints to exceed their limits.  To avoid this calamity, the servo
will modify the set point. Each joint has two limits to its motion;
call these L1 and L2.  There will be a "warning track" close to
these limits; if the joint enters the warning track, it will be
deflected in such a way that it can never hit the limit.  The
warning tracks begin at W1 and W2.  The set point is calculated
as follows:

S ← IF S > W2 
	THEN L2 - (L2-W2)↑2/((L2-W2)+(S-W2))
    ELSE IF S < W1
	THEN L1 - (L1-W1)↑2/((L1-W1)+(S-W1))
    ELSE S


Here it is assumed that W2 is the high track, and W1 marks the low one.
Note that much of this expression is built of constants.